/// **********************************************************************
//                            Project Nitro
//                        ---------------------
//
//  File        : Car.cpp
//  Description : Car Class
//  Author      : Guillaume Delhumeau <guillaume.delhumeau@gmail.com>
//  License     : GNU General Public License 3
//
// **********************************************************************

#include "Car.h"
#include "Tools.h"
#include <cmath>

/************************************************************************/
/* Constants                                                            */
/************************************************************************/
static const real PI =         3.1415926535897932f;
static const real DEG_TO_RAD = 0.01745329251994329576923f;
static const real RAD_TO_DEG = 57.29577951308232087679815481f;
static const CL_Angle WHEEL_MAX_ROTATION(35.0f, cl_degrees);
static const real WHEEL_ROTATION_SPEED = 90.0f;
static const real WHEEL_RETOR = 240.0f;
static const real CAR_ROTATION_SPEED = 0.5f;
static const real MINIMUM_VELOCITY = 35;
static const float SEUIL_SENSIBILITE_JOYSTICK = 0.5f;

static const real AIR_RESISTANCE = 0.42f;
static const real WHEEL_RESISTANCE = 12.6f;
static const real ENGINE_FORCE = 200000;
static const real BRAKE_FORCE = 5000.0f;

static const int L = 17; // 27

/************************************************************************/
/* Constructor                                                          */
/************************************************************************/
Car::Car():
_brake(0)
{
	_gear = Car::NONE;
	_car_rotation.set_degrees(0);
	_wheel_rotation.set_degrees(0);
	_car_matrix.SetIdentity();
	_car_matrix.SetPos(400, 0, -300, 1);
	_v_velocity.Set(0, 0, 0, 0);
	_velocity = 0;
}

/************************************************************************/
/* Set resources                                                        */
/************************************************************************/
void Car::set_resources(CarResources & resources)
{
	my_assert(resources.is_loaded(), "Car resources must be loaded");
	_p_car_resources = & resources;
}

/************************************************************************/
/* Draw                                                                 */
/************************************************************************/
void Car::draw(Screen & screen, float x, float y)
{
	my_assert(_p_car_resources != NULL, "Car Resources must be not null.");

	// Set rotations
	CL_Angle car_angle(-_car_rotation.to_degrees(), cl_degrees);
	_p_car_resources->get_body().set_angle(car_angle);
	CL_Angle wheel_angle(-_car_rotation.to_degrees() - _wheel_rotation.to_degrees(), cl_degrees);
	_p_car_resources->get_wheel().set_angle(wheel_angle);

	// Draw
	screen.draw(_p_car_resources->get_body(), x, y);

	// Draw wheels
	Vector4f wheel_pos_left;
	wheel_pos_left.Set(-17, 0, 11, 1);
	wheel_pos_left = _car_matrix * wheel_pos_left -_car_matrix.GetPos();
	screen.draw(_p_car_resources->get_wheel(), x + wheel_pos_left.GetX(), y - wheel_pos_left.GetZ());

	Vector4f wheel_pos_right;
	wheel_pos_right.Set(16, 0, 11, 1);
	wheel_pos_right = _car_matrix * wheel_pos_right -_car_matrix.GetPos();	
	screen.draw(_p_car_resources->get_wheel(), x + wheel_pos_right.GetX(), y - wheel_pos_right.GetZ());
		
}

/************************************************************************/
/* Calculate car speed                                                  */
/************************************************************************/
void Car::_compute_velocity(float time_elapsed)
{
	real mass = 1000.0f;

	real air_resistance = - AIR_RESISTANCE * _velocity * _velocity;
	real wheel_resistance = - WHEEL_RESISTANCE * _velocity;
	real brake = - _brake * _velocity;
	real total_forces = _traction + air_resistance + wheel_resistance + brake;
	real accel = total_forces / mass;
	_velocity = _velocity + accel * time_elapsed;
	if(_velocity <= MINIMUM_VELOCITY)
	{
		if(accel <= 0)
			_velocity = 0;
		else
			_velocity = MINIMUM_VELOCITY;
	}
}

/************************************************************************/
/* Turning at low speed                                                 */
/************************************************************************/
void Car::_turn(Turning turning, real time_elapsed)
{
	// I need the circle center
	CL_Angle alpha(_wheel_rotation);
	if(alpha.to_degrees() < 0)
		alpha.set_degrees(-alpha.to_degrees());

	float radius = (float)L / tan(alpha.to_radians()) +  _velocity * .25f;
	if(radius > 1000)
		radius = 1000;
	Vector4f circle_center;
	if(turning == LEFT)
		circle_center.Set(- 18 - radius, 0, -16, 1);
	else
		circle_center.Set(18 + radius, 0, -16, 1);	
	circle_center = _car_matrix * circle_center;

// 	sf::Shape circle = sf::Shape::Circle(circle_center.GetX(), 
// 		600 - circle_center.GetZ(), 5, sf::Color(0, 128, 128)	);
	//_render_target.Draw(circle);

	// polair coords
	Vector4f car_pos = _car_matrix.GetPos();
	Vector4f circle_to_car = car_pos - circle_center;
	float rho = circle_to_car.GetLength();
	float x = circle_to_car.GetX();
	CL_Angle theta(acosf(x / rho), cl_radians);
	float ztest = circle_center.GetZ() + rho * sin(theta.to_radians());
	if(abs(ztest - car_pos.GetZ()) > 1)
	{
		theta.set_radians(-theta.to_radians());			
	}

	// Angular speed
	CL_Angle angular_speed(_velocity / (radius + 32), cl_radians);

	// New coords
	CL_Angle delta_theta(angular_speed.to_radians() * time_elapsed, cl_radians);

	if( (turning == RIGHT && _gear == FORWARD)
		|| (turning == LEFT && _gear == BACKWARD))
		delta_theta.set_degrees(-delta_theta.to_degrees());

	CL_Angle new_theta(theta + delta_theta);
	float newX = circle_center.GetX() + rho * cos(new_theta.to_radians());
	float newZ = circle_center.GetZ() + rho * sin(new_theta.to_radians());


	Vector4f d;
	if(_gear == FORWARD)
		d.Set(0, 0, _velocity * time_elapsed, 0);
	else
		d.Set(0, 0, - _velocity * time_elapsed, 0);

	Vector4f old_pos = _car_matrix.GetPos();

	_car_rotation += delta_theta;
	Matrix4f mat_rot;
	mat_rot.SetRotationY( delta_theta.to_radians() );
	_car_matrix = _car_matrix * mat_rot;

	_car_matrix.SetPos(newX, 0 , newZ, 1);


	_v_velocity = _car_matrix.GetPos() - old_pos;
	_v_velocity.Normalize();
}

/************************************************************************/
/* Going straight                                                       */
/************************************************************************/
void Car::_go_straight(real time_elapsed)
{
	real delta_z = _velocity * time_elapsed;
	if(_gear == BACKWARD)
		delta_z = -delta_z;
	Vector4f delta;
	delta.Set(0, 0, delta_z, 0);
	delta = _car_matrix * delta;
	Vector4f new_pos = _car_matrix.GetPos() + delta;
	_car_matrix.SetPos(new_pos);

// 	int MODULO_ANGLE = 20;
// 	int modulo = (int)_car_rotation.to_degrees()%MODULO_ANGLE;
// 	if(modulo > 0)
// 	{
// 		CL_Angle delta_angle(-modulo, cl_degrees);
// 		if(_last_turning == Turning::LEFT)
// 		{
// 			_car_rotation.set_degrees(_car_rotation.to_degrees()-modulo);
// 		}
// 		else
// 		{
// 			_car_rotation.set_degrees(_car_rotation.to_degrees()+MODULO_ANGLE-modulo);
// 			delta_angle.from_degrees(MODULO_ANGLE-modulo);
// 		}
// 		Matrix4f mat_rot;
// 		mat_rot.SetRotationY( delta_angle.to_radians() );
// 		_car_matrix = _car_matrix * mat_rot;
// 	}
}

/************************************************************************/
/* Update Car                                                           */
/************************************************************************/
void Car::update(Screen & screen)
{
	float time_elapsed = screen.get_time_elapsed();

	if(time_elapsed > 1)
		time_elapsed = 1;
	if(time_elapsed < 0)
		time_elapsed = 0;

	Vector4f old_pos = _car_matrix.GetPos();

	_compute_velocity(time_elapsed);
	_car_rotation.normalize();

	// TURN LEFT
	if(_wheel_rotation.to_degrees() > 0 && _velocity > 0.1)
	{
		_turn(LEFT, time_elapsed);		
	}
	// TURN RIGHT
	else if(_wheel_rotation.to_degrees() < 0 && _velocity > 0.1)
	{
		_turn(RIGHT, time_elapsed);
	}
	// GO STRAIGTH
	else
	{
		_go_straight(time_elapsed);
	}
	Vector4f new_velocity = _car_matrix.GetPos() - old_pos;

	// Calc grip;
	_grip = 1;// - _velocity / 100;
	if(_grip > 1)
		_grip = 1;
	else if(_grip < 0.1)
		_grip = 0.1f;

	_v_velocity = _grip * new_velocity + (1.0f - _grip) * _v_velocity.GetNormalized() * _velocity * time_elapsed;

	Vector4f new_pos = old_pos + _v_velocity;
	_car_matrix.SetPos(new_pos);
}

/************************************************************************/
/* Car events (inputs)                                                  */
/************************************************************************/
void Car::events(Screen & screen)
{
	const CL_InputDevice & keyboard = screen.get_ic().get_keyboard();

	bool key_left = keyboard.get_keycode(CL_KEY_LEFT);
	bool key_right = keyboard.get_keycode(CL_KEY_RIGHT);
	bool key_up = keyboard.get_keycode(CL_KEY_UP);
	bool key_down = keyboard.get_keycode(CL_KEY_DOWN);
	
	if(screen.get_ic().get_joystick_count()>0)
	{
		const CL_InputDevice & joystick = screen.get_ic().get_joystick();

		key_left = key_left || joystick.get_axis(0)<-SEUIL_SENSIBILITE_JOYSTICK;
		key_right = key_right || joystick.get_axis(0)>SEUIL_SENSIBILITE_JOYSTICK;
		key_up = key_up || joystick.get_keycode(1);
		key_down = key_down || joystick.get_keycode(0);
	}

	if(keyboard.get_keycode(CL_KEY_RETURN))
	{
		_car_rotation.set_degrees(0);
		_wheel_rotation.set_degrees(0);
		_car_matrix.SetIdentity();
		_car_matrix.SetPos(400, 0, -300.0f, 1);
		_traction = 0;
		_velocity = 0;
		_brake = 0;
		_gear = NONE;
		_v_velocity.Set(0, 0, 0, 0);
	}

	if(key_up)
	{
		switch(_gear)
		{
		case FORWARD:
			_traction = ENGINE_FORCE;
			break;
		case NONE:
			_traction = ENGINE_FORCE;
			_gear = FORWARD;
			break;
		case BACKWARD:
			_brake = BRAKE_FORCE;
			break;
		}	
	}	
	else
	{
		switch(_gear)
		{
		case FORWARD:
			_traction = 0;
			break;
		case NONE:
			_traction = 0;
			break;
		case BACKWARD:
			_brake = 0;
			if(_velocity < 5)
				_gear = NONE;
			break;
		}	
	}

	if(key_down)
	{
		switch(_gear)
		{
		case FORWARD:
			_brake = BRAKE_FORCE;
			break;
		case NONE:
			_traction = ENGINE_FORCE;
			_gear = BACKWARD;
			break;
		case BACKWARD:
			_traction = ENGINE_FORCE;
			break;
		}	
	}	
	else
	{
		switch(_gear)
		{
		case FORWARD:
			_brake = 0;
			if(_velocity < 5)
				_gear = NONE;
			break;
		case NONE:
			_brake = 0;
			break;
		case BACKWARD:
			_traction = 0;
			break;
		}	
	}

	if(key_left)
	{
		CL_Angle alpha(WHEEL_ROTATION_SPEED * screen.get_time_elapsed(), cl_degrees);		
		_wheel_rotation += alpha;
		if(_wheel_rotation.to_degrees() > WHEEL_MAX_ROTATION.to_degrees())
			_wheel_rotation = WHEEL_MAX_ROTATION;		
		_last_turning = LEFT;
	}
	else if(_wheel_rotation.to_degrees() > 0)
	{
		CL_Angle alpha(-WHEEL_RETOR * screen.get_time_elapsed(), cl_degrees);		
		_wheel_rotation += alpha;
		if(_wheel_rotation.to_degrees() < 0)
			_wheel_rotation.set_degrees(0);		
	}

	if(key_right)
	{
		CL_Angle alpha(-WHEEL_ROTATION_SPEED * screen.get_time_elapsed(), cl_degrees);
		_wheel_rotation += alpha;
		if(_wheel_rotation.to_degrees() < -WHEEL_MAX_ROTATION.to_degrees())
			_wheel_rotation.set_degrees(-WHEEL_MAX_ROTATION.to_degrees());
		_last_turning = RIGHT;
	}
	else if(_wheel_rotation.to_degrees() < 0)
	{
		CL_Angle alpha(WHEEL_RETOR * screen.get_time_elapsed(), cl_degrees);
		_wheel_rotation += alpha;
		if(_wheel_rotation.to_degrees() > 0)
			_wheel_rotation.set_degrees(0);		
	}
}
